#include "global.h"

/*============================================================================*/
/*                           飞行控制核心参数配置                             */
/*============================================================================*/

/***************************** 遥控器通道参数定义 ****************************/
#define TXREMOTE_CHANNEL_LOW 1000         // 通道低位值（数值1000）
#define TXREMOTE_CHANNEL_HALF 1500        // 通道中位值（数值1500）
#define TXREMOTE_CHANNEL_HIGH 2000        // 通道高位值（数值2000）
#define TXREMOTE_THROTTLE_ARM_MAX 1050    // 解锁允许的最大油门值（摇杆需置于最低位，数值<1050）
#define TXREMOTE_THROTTLE_ACTIVE_MIN 1200 // 进入飞行状态的最小油门值（摇杆需抬升到数值>1200）

/***************************** 飞行安全边界参数 ******************************/
#define TILT_ANGLE_CRITICAL_LIMIT 50.0f // 最大允许姿态角（超过±50°触发紧急保护措施）

/***************************** 状态迁移时间阈值 ******************************/
#define TIMEOUT_ARMING_UNLOCK COUNTER3S            // 解锁确认时间（需保持3秒）
#define TIMEOUT_ARMED_AUTO_LOCK COUNTER5S          // 解锁后自动锁定时间（5秒内未起飞则重新锁定）
#define TIMEOUT_ACTIVE_MANUALSHUTDOWN COUNTER1S    // 手动急停触发时间（需保持1秒）
#define TIMEOUT_ACTIVE_LANDING_LOCK COUNTER10S     // 着陆锁定时间（油门低位10秒自动锁定）
#define TIMEOUT_ACTIVE_TILTPROTECTION COUNTER500MS // 姿态保护触发时间（异常姿态持续500ms触发保护）
#define TIMEOUT_EMERGENCY_UNLOCK COUNTER3S         // 紧急状态解除时间（需长按3秒退出紧急状态）

/*============================== 函数实现 ==================================*/

/**
 * @brief 设置所有电机的速度
 * @param speed 电机PWM数值（0-1000范围）
 * @note 同时设置四个电机至相同速度，用于起飞、降落或紧急停机
 */
static void motor_setSpeedAll(uint16_t speed)
{
    pwmMotor_setCompare1(speed);
    pwmMotor_setCompare2(speed);
    pwmMotor_setCompare3(speed);
    pwmMotor_setCompare4(speed);
}

/**
 * @brief 设置所有LED的显示模式
 * @param mode LED显示模式（如呼吸、闪烁等）
 * @note 通过LED提示当前飞行器状态
 */
void led_setModeAll(_em_LEDMode mode)
{
    led_setMode(LED_1, mode);
    led_setMode(LED_2, mode);
    led_setMode(LED_3, mode);
    led_setMode(LED_4, mode);
}

/**
 * @brief 遥控器解锁条件验证
 * @return 1-解锁条件满足 0-条件不满足
 * @note 安全解锁需同时满足：AUX1通道为高位(2000) + 油门低于1050
 */
static uint8_t isArmingConditionMet()
{
    return (txRemote.aux1 == TXREMOTE_CHANNEL_HIGH && txRemote.thr < TXREMOTE_THROTTLE_ARM_MAX);
}

/**
 * @brief 姿态安全检测
 * @return 1-姿态危险 0-姿态正常
 * @note 检测横滚角和俯仰角是否超过安全限值
 */
static uint8_t isTiltExceedLimit()
{
    return (fabs(imuEulerAngle.roll) > TILT_ANGLE_CRITICAL_LIMIT ||
            fabs(imuEulerAngle.pitch) > TILT_ANGLE_CRITICAL_LIMIT);
}

/*------------------------------- 飞行状态处理 ----------------------------*/

/**
 * @brief 处理锁定状态
 * @param timeoutCounter 计时器指针
 * @return 下一个飞行状态
 */
static _em_FlightState handleDisarmedState(uint16_t *timeoutCounter)
{
    if (isArmingConditionMet())
    {
        *timeoutCounter = 0;
        return ARMING;
    }
    return DISARMED;
}

/**
 * @brief 处理解锁中状态
 * @param timeoutCounter 计时器指针
 * @return 下一个飞行状态
 * @note 需持续满足解锁条件3秒才能完成解锁
 */
static _em_FlightState handleArmingState(uint16_t *timeoutCounter)
{
    if (isArmingConditionMet())
    {
        if (++(*timeoutCounter) >= TIMEOUT_ARMING_UNLOCK)
        {
            *timeoutCounter = 0;
            return ARMED;
        }
    }
    else
    {
        return DISARMED;
    }
    return ARMING;
}

/**
 * @brief 处理已解锁状态
 * @param timeoutCounter 计时器指针
 * @param shutdownCounter 急停计时器指针
 * @param tiltCounter 姿态异常计时器指针
 * @return 下一个飞行状态
 * @note 解锁后等待起飞，若5秒内未起飞则自动锁定
 */
static _em_FlightState handleArmedState(uint16_t *timeoutCounter, uint16_t *shutdownCounter, uint16_t *tiltCounter)
{
    if (txRemote.thr > TXREMOTE_THROTTLE_ACTIVE_MIN)
    {
        imu_resetYaw(); // 重置偏航角
        // 重置所有PID控制器，消除积分项累积
        pid_rest(&pidPitch);
        pid_rest(&pidRoll);
        pid_rest(&pidYaw);
        pid_rest(&pidRateX);
        pid_rest(&pidRateY);
        pid_rest(&pidRateZ);
        *timeoutCounter = *shutdownCounter = *tiltCounter = 0; // 重置计时器
        return ACTIVE;
    }
    else if (++(*timeoutCounter) >= TIMEOUT_ARMED_AUTO_LOCK)
    {
        return DISARMED;
    }
    return ARMED;
}

/**
 * @brief 处理飞行中状态
 * @param timeoutCounter 计时器指针
 * @param shutdownCounter 急停计时器指针
 * @param tiltCounter 姿态异常计时器指针
 * @return 下一个飞行状态
 * @note 进行PID控制及姿态监测，包含多种保护机制
 */
static _em_FlightState handleActiveState(uint16_t *timeoutCounter, uint16_t *shutdownCounter, uint16_t *tiltCounter)
{
    float motor1, motor2, motor3, motor4;

    // 手动急停检测 - AUX1高位
    if (txRemote.aux1 == TXREMOTE_CHANNEL_HIGH)
    {
        if (++(*shutdownCounter) >= TIMEOUT_ACTIVE_MANUALSHUTDOWN)
        {
            *timeoutCounter = 0;
            return EMERGENCY;
        }
    }
    else
    {
        *shutdownCounter = 0;
    }

    // 姿态异常检测 - 倾角超过限制
    if (isTiltExceedLimit())
    {
        if (++(*tiltCounter) >= TIMEOUT_ACTIVE_TILTPROTECTION)
        {
            *timeoutCounter = 0;
            return EMERGENCY;
        }
    }
    else
    {
        *tiltCounter = 0;
    }

    // 着陆检测 - 油门处于最低位
    if (txRemote.thr < TXREMOTE_THROTTLE_ARM_MAX)
    {
        if (++(*timeoutCounter) >= TIMEOUT_ACTIVE_LANDING_LOCK)
        {
            return DISARMED;
        }
    }
    else
    {
        *timeoutCounter = 0;
    }

    // 横滚角PID控制
    pidRoll.target = -(txRemote.roll - TXREMOTE_CHANNEL_HALF) * 0.04f; // 横滚角目标值
    pidRoll.measured = imuEulerAngle.roll;                             // 测量值：当前横滚角
    pid_update(&pidRoll, dt);                                          // 计算横滚角PID
    pidRateX.target = pidRoll.out;                                     // 将横滚角PID输出作为X轴角速度目标值
    pidRateX.measured = mpu6050_F.gyroX;                               // 测量值：X轴角速度
    pid_update(&pidRateX, dt);                                         // 计算X轴角速度PID

    // 俯仰角PID控制
    pidPitch.target = -(txRemote.pitch - TXREMOTE_CHANNEL_HALF) * 0.04f; // 俯仰角目标值
    pidPitch.measured = imuEulerAngle.pitch;                             // 测量值：当前俯仰角
    pid_update(&pidPitch, dt);                                           // 计算俯仰角PID
    pidRateY.target = pidPitch.out;                                      // 将俯仰角PID输出作为Y轴角速度目标值
    pidRateY.measured = mpu6050_F.gyroY;                                 // 测量值：Y轴角速度
    pid_update(&pidRateY, dt);                                           // 计算Y轴角速度PID

    // 偏航角PID控制
    if (txRemote.yaw < 1200) // 偏航控制：左右打杆控制旋转方向
    {
        pidYaw.target += 0.4f; // 遥控器左打杆，向左旋转
    }
    else if (txRemote.yaw > 1800)
    {
        pidYaw.target -= 0.4f; // 遥控器右打杆，向右旋转
    }
    if (pidYaw.target > 180.0f) // 限制偏航角目标值在[-180, 180]范围内
    {
        pidYaw.target -= 360.0f;
    }
    else if (pidYaw.target < -180.0f)
    {
        pidYaw.target += 360.0f;
    }
    pidYaw.measured = imuEulerAngle.yaw;                                // 测量值：当前偏航角
    pidYaw.error = wrapAngleDifference(pidYaw.target, pidYaw.measured); // 计算偏航角误差
    pid_update_E(&pidYaw, dt);                                          // 计算偏航角PID
    pidRateZ.target = pidYaw.out;                                       // 将偏航角PID输出作为Z轴角速度目标值
    pidRateZ.measured = mpu6050_F.gyroZ;                                // 测量值：Z轴角速度
    pid_update(&pidRateZ, dt);                                          // 计算Z轴角速度PID

    // 油门值映射到电机转速
    motor1 = (txRemote.thr - TXREMOTE_CHANNEL_LOW) * 0.7f; // 基础油门值
    motor2 = motor1;
    motor3 = motor1;
    motor4 = motor1;

    // 根据PID输出调整各电机转速，实现姿态控制
    motor1 += +pidRateX.out + pidRateY.out + pidRateZ.out; // 前左电机
    motor2 += +pidRateX.out - pidRateY.out - pidRateZ.out; // 前右电机
    motor3 += -pidRateX.out + pidRateY.out - pidRateZ.out; // 后左电机
    motor4 += -pidRateX.out - pidRateY.out + pidRateZ.out; // 后右电机

    // 电机输出限幅，确保转速在[0.0, 999.0]范围内
    LIMIT_F(&motor1, 0.0f, 999.0f);
    LIMIT_F(&motor2, 0.0f, 999.0f);
    LIMIT_F(&motor3, 0.0f, 999.0f);
    LIMIT_F(&motor4, 0.0f, 999.0f);

    // 更新电机输出至PWM控制器
    pwmMotor_setCompare1((uint16_t)motor1);
    pwmMotor_setCompare2((uint16_t)motor2);
    pwmMotor_setCompare3((uint16_t)motor3);
    pwmMotor_setCompare4((uint16_t)motor4);
    return ACTIVE;
}

/**
 * @brief 处理紧急状态
 * @param timeoutCounter 计时器指针
 * @return 下一个飞行状态
 * @note 需通过AUX2高位3秒才能解除紧急状态
 */
static _em_FlightState handleEmergencyState(uint16_t *timeoutCounter)
{
    if (txRemote.aux2 == TXREMOTE_CHANNEL_HIGH)
    {
        if (++(*timeoutCounter) >= TIMEOUT_EMERGENCY_UNLOCK)
        {
            return DISARMED;
        }
    }
    else
    {
        *timeoutCounter = 0;
    }
    return EMERGENCY;
}

/*============================== 主状态机实现 ================================*/
/**
 * @brief 飞行控制主函数，包含状态机逻辑
 * @note 每个控制周期调用一次，处理状态迁移和控制逻辑
 */
void flightControl(void)
{
    static uint16_t timeoutCounter = 0;             // 通用状态计时器
    static uint16_t shutdownCounter = 0;            // 急停触发计时器
    static uint16_t tiltCounter = 0;                // 姿态异常持续计时
    static _em_FlightState previousState = UNKNOWN; // 状态变更检测
    _em_FlightState nextState = flightState;        // 状态迁移变量

    /*------------------------- 遥控器信号完整性检查 ------------------------*/
    if (rxRemote_analysis() != SUCCESS)
    {
        timeoutCounter = 0;
        nextState = EMERGENCY; // 遥控信号异常立即进入紧急状态
    }

    /*============================== 状态迁移逻辑 ===========================*/
    switch (nextState)
    {
    case DISARMED:
        nextState = handleDisarmedState(&timeoutCounter);
        break;
    case ARMING:
        nextState = handleArmingState(&timeoutCounter);
        break;
    case ARMED:
        nextState = handleArmedState(&timeoutCounter, &shutdownCounter, &tiltCounter);
        break;
    case ACTIVE:
        nextState = handleActiveState(&timeoutCounter, &shutdownCounter, &tiltCounter);
        break;
    case EMERGENCY:
        nextState = handleEmergencyState(&timeoutCounter);
        break;
    default:
        nextState = EMERGENCY; // 未知状态进入紧急模式
        break;
    }

    /*---------------------------- 状态变化反馈 --------------------------*/
    if (nextState != previousState)
    {
        switch (nextState)
        {
        case DISARMED:
            motor_setSpeedAll(0);           // 电机停转
            led_setModeAll(LEDMODE_BREATH); // LED呼吸模式
            break;
        case ARMING:
            led_setModeAll(LEDMODE_BREATH); // LED呼吸模式
            break;
        case ARMED:
            led_setModeAll(LEDMODE_ON); // LED常亮
            break;
        case ACTIVE:
            led_setModeAll(LEDMODE_DOUBLE_BLINK); // LED双闪
            break;
        case EMERGENCY:
            motor_setSpeedAll(0);          // 电机紧急停转
            led_setModeAll(LEDMODE_BLINK); // LED快闪
            break;
        default:
            motor_setSpeedAll(0);          // 电机紧急停转
            led_setModeAll(LEDMODE_BLINK); // LED快闪
            break;
        }
        previousState = nextState;
    }

    flightState = nextState; // 更新当前状态
}
